/*============================================================================*/
/*                     Motores - EMBEDDED SYSTEMS SOFTWARE                       */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:        Motores.c
* version:         1.0 
* created_by:      Panters 
* date_created:    Sep 15 2013 
*=============================================================================*/
/* DESCRIPTION : C source for Timer/PWM Module management                     */
/*============================================================================*/
/* FUNCTION COMMENT : This file describes the C source template according to  */
/* the new software platform                                                  */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 15/09/2013  |                               | David Robles     */
/* First version of this source                                               */
/*============================================================================*/

/* Includes */

#include "SCR/TPM.h"
/* -------- */

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 
/* BYTE constants */

/* WORD constants */

/* LONG and STRUCTURE constants */

/* Custom type constants */


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/ 
/* BYTE RAM variables */

/* WORD RAM variables */

/* LONG and STRUCTURE RAM variables */
T_UWORD ruw_Servo_3010Ftb_CmpValtoPWM;

/* Custom type RAM variables */

/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */
#define FTM1_CLOCK                  24000000u
#define FTM1_CLK_PRESCALE          6u// = 64 Prescale Selector value - see comments in Status Control (SC) section for more details
#define FTM1_OVERFLOW_FREQUENCY 50u  // Desired Frequency of PWM Signal - Here 50Hz => 20ms period


#define TFC_MOTOR_SWITCHING_FREQUENCY	4000u
#define FTM0_CLOCK                                   	      24000000u
#define FTM0_CLK_PRESCALE                                 	   0  // Prescale Selector value - see comments in Status Control (SC) section for more details
#define FTM0_OVERFLOW_FREQUENCY 5000							  //
#define FTM0_MOD_VALUE	FTM0_CLOCK/TFC_MOTOR_SWITCHING_FREQUENCY

// (24000000Hz/64)/50Hz = 7500
// TPM period = TPMx_MOD + 1; FOr this case is 7501; 7501/(24000000Hz/64) = 0.02000269167 s
#define TPM1_MOD_VAL	(FTM1_CLOCK/(1<<(FTM1_CLK_PRESCALE)))/FTM1_OVERFLOW_FREQUENCY
#define RES_CALC			10 // Resolution for calculus
#define _100_PERCENT_CALC	100 * RES_CALC
#define MIN_DC_SERVO	52//52// = 5% at RES_CALC
#define MAX_DC_SERVO	93//90 // = 10% at RES_CALC  88
#define CENTER_CMPVAL_SERVO 540
#define MIN_CMPVAL_SERVO	410 // = TPM1_MOD_VAL * 5% / 100
#define MAX_CMPVAL_SERVO	670 // = TPM1_MOD_VAL * 10% / 100

/* Private functions prototypes */
/* ---------------------------- */


/* Exported functions prototypes */
/* ----------------------------- */

/* Inline functions */
/* ---------------- */
/**************************************************************
 *  Name                 : inline_func	2
 *  Description          :
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :
 *  Critical/explanation :    [yes / No]
 **************************************************************/


/* Private functions */
/* ----------------- */
/**************************************************************
 *  Name                 : private_func
 *  Description          :
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :
 *  Critical/explanation :    [yes / No]
 **************************************************************/


/* Exported functions */
/* ------------------ */
/**************************************************************
 *  Name                 :	export_func
 *  Description          :
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :
 *  Critical/explanation :    [yes / No]
 **************************************************************/
void TPM_general_init (void)
{
	/* Clock Setup for all TPMs */
	{
		/* Provide clock for the TPM module (Section 5.7.5 TPM clocking from "KL25 Sub-Family Reference Manual") */
		/* Select MCGPLLCLK/2 as clock source in SIM_SOPT2 (Section 12.2.3 System Options Register 2 (SIM_SOPT2)  from "KL25 Sub-Family Reference Manual") */
		SIM_SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK;
		/* Clear previous configuration of clock selected for TPM (Section 12.2.3 System Options Register 2 (SIM_SOPT2)  from "KL25 Sub-Family Reference Manual") */
		SIM_SOPT2 &= ~(SIM_SOPT2_TPMSRC_MASK);
		/* Select finally the source for TPM. Clock source provided by MCGPLLCLK/2  (Section 12.2.3 System Options Register 2 (SIM_SOPT2) from "KL25 Sub-Family Reference Manual") */
		/* 01 MCGFLLCLK clock or MCGPLLCLK/2 */
		SIM_SOPT2 |= SIM_SOPT2_TPMSRC(1);		
	}
	
	/* Enable/Disable clock for each TPM instance */
	{
		/* See Section 12.2.10 System Clock Gating Control Register 6 (SIM_SCGC6) from "KL25 Sub-Family Reference Manual" */
		/* Enable clock for TPM1 */
		SIM_SCGC6 |= SIM_SCGC6_TPM1_MASK;
		/* Enable clock for TPM0 */
		SIM_SCGC6 |= SIM_SCGC6_TPM0_MASK;
	}

}

/**************************************************************
 *  Name                 :	export_func
 *  Description          :
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :
 *  Critical/explanation :    [yes / No]
 **************************************************************/
void TPM_init_TPM1(void)
{
    //Blow away the control registers to ensure that the counter is not running
	// Disable TPM1 to configure
    TPM1_SC = 0;
    TPM1_CONF = 0;
    
    //While the counter is disabled we can setup the prescaler
    TPM1_SC |= TPM_SC_PS(FTM1_CLK_PRESCALE);
    //TPM1_SC |= TPM_SC_TOIE_MASK; //Enable Interrupts for the Timer Overflow
    
    //Setup the mod register to get the correct PWM Period
    
    TPM1_MOD = TPM1_MOD_VAL;
    
    //Setup Channel 0
    /* Edge-aligned PWM; clear Output on match, set Output on reload */
    TPM1_C0SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
    //TPM1_C1SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
                   
	//Set the Default duty cycle
    TPM1_C0V = CENTER_CMPVAL_SERVO;
	//TFC_SetServo(0, 0.0);
	//TFC_SetServo(1, 0.0);
	
	//Enable the TPM COunter
	TPM1_SC |= TPM_SC_CMOD(1);
	
	//Enable TPM1 IRQ on the NVIC
	//enable_irq (INT_TPM1-16);
	
	//Enable the FTM functions on the the port
	/* Turn on clock to PortB module */
	SIM_SCGC5 |= SIM_SCGC5_PORTB_MASK;
	PORTB_PCR0 |= PORT_PCR_MUX(3);
	//PORTB_PCR1 = PORT_PCR_MUX(3);
}

/**************************************************************
 *  Name                 :	export_func
 *  Description          :
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :
 *  Critical/explanation :    [yes / No]
 **************************************************************/
void TPM_init_TPM0 (void)
{
	
	
	
	
	
	//Blow away the control registers to ensure that the counter is not running
	    TPM0_SC = 0;
	    TPM0_CONF = 0;
	    
	    //While the counter is disabled we can setup the prescaler
	    
	    TPM0_SC = TPM_SC_PS(FTM0_CLK_PRESCALE);
	    
	    //Setup the mod register to get the correct PWM Period
	    
	    TPM0_MOD = FTM0_CLOCK/(1<<FTM0_CLK_PRESCALE)/FTM0_OVERFLOW_FREQUENCY;
	    
	    //Setup Channels 0,1,2,3
	    TPM0_C0SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
	    TPM0_C1SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; // invert the second PWM signal for a complimentary output;
	    TPM0_C2SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
	    TPM0_C3SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; // invert the second PWM signal for a complimentary output;
	    
	    
	    //Enable the Counter
	    
	    //Set the Default duty cycle to 50% duty cycle
	    
	    
	    //Enable the TPM COunter
	    TPM0_SC |= TPM_SC_CMOD(1);
	    
	    
	    //Enable the FTM functions on the the port
	    PORTC_PCR1 = PORT_PCR_MUX(4);
	    PORTC_PCR2 = PORT_PCR_MUX(4);     
	    PORTC_PCR3 = PORT_PCR_MUX(4);  
	    PORTC_PCR4 = PORT_PCR_MUX(4);  

}

void Set_servo_position_0 (T_UBYTE position)
{
	T_ULONG temp1 = 0;
	T_ULONG temp2 = 0;
	// la posision se controla con un DC de 5% a 10%
	// se usara resolucion de 0.1 para mayor presicion (punto fijo)
	
	if(position > MAX_DC_SERVO)
	{
		position = MAX_DC_SERVO;
	}
	else if (position < MIN_DC_SERVO)
	{
		position = MIN_DC_SERVO;
	}
	
	// Convertir a valores adecuados para los registros del TPM
	temp2 = (TPM1_MOD_VAL + 1) * RES_CALC;
	temp1 = position * temp2;
	temp2 = temp1 / (_100_PERCENT_CALC);
	temp1 = temp2 / RES_CALC;
	
	if(temp1 > MAX_CMPVAL_SERVO)
	{
		ruw_Servo_3010Ftb_CmpValtoPWM = MAX_CMPVAL_SERVO;
	}
	else if(temp1 < MIN_CMPVAL_SERVO)
	{
		ruw_Servo_3010Ftb_CmpValtoPWM = MIN_CMPVAL_SERVO;
	}
	else
	{
		ruw_Servo_3010Ftb_CmpValtoPWM = temp1;
	}
	
	
	TPM1_C0V = ruw_Servo_3010Ftb_CmpValtoPWM;
}

void Set_servo_position (T_UWORD position)
{
	if(position > MAX_CMPVAL_SERVO)
	{
		ruw_Servo_3010Ftb_CmpValtoPWM = MAX_CMPVAL_SERVO;
	}
	else if(position < MIN_CMPVAL_SERVO)
	{
		ruw_Servo_3010Ftb_CmpValtoPWM = MIN_CMPVAL_SERVO;
	}
	else
	{
		ruw_Servo_3010Ftb_CmpValtoPWM = position;
	}
	
	TPM1_C0V = ruw_Servo_3010Ftb_CmpValtoPWM;
}

void Servo_control_0 (void)
{
	PRIVATE_DATA T_UBYTE temp_DC = 75;
	PRIVATE_DATA E_BOOLEAN temp_flag = BFALSE;
	
	Set_servo_position(temp_DC);
	
	if(temp_flag == BFALSE)
	{
		temp_DC += 5;
		if(temp_DC >= MAX_DC_SERVO)
		{
			temp_DC = MAX_DC_SERVO;
			temp_flag = BTRUE;
		}
	}
	else
	{
		temp_DC -= 5;
		if(temp_DC <= MIN_DC_SERVO)
		{
			temp_DC = MIN_DC_SERVO;
			temp_flag = BFALSE;
		}
	}
	
}

void Servo_control_1 (void)
{
	PRIVATE_DATA T_UWORD temp_DC = CENTER_CMPVAL_SERVO;
	PRIVATE_DATA E_BOOLEAN temp_flag = BFALSE;
	
	Set_servo_position(temp_DC);
	
	if(temp_flag == BFALSE)
	{
		temp_DC += 1;
		if(temp_DC >= MAX_CMPVAL_SERVO)
		{
			temp_DC = MAX_CMPVAL_SERVO;
			temp_flag = BTRUE;
		}
	}
	else
	{
		temp_DC -= 1;
		if(temp_DC <= MIN_CMPVAL_SERVO)
		{
			temp_DC = MIN_CMPVAL_SERVO;
			temp_flag = BFALSE;
		}
	}
}




void Motor_control(volatile float MotorA,volatile float MotorB,volatile char DirA,volatile char DirB)
{
	
	/*Checa si la condicion no rebasa los parametros de DC*/
	
	if(MotorA>100)
	{
		MotorA=100;
	}
	else if((MotorA==0)|( MotorA<0) )
	{
		MotorA=1;
	}

	if(MotorB>100)
	{
		MotorB=100;
	}
	else if((MotorB==0) |(MotorB<0))
	{
		MotorB=1;
	}
	
	/*operacion necesaria para controlar DC*/
	
	MotorA*=0.01;
	MotorB*=0.01;
	
	
	
	
	/*Direccion de los motores*/
	
	if(DirA==1)
	{
		TPM0_C2V = (uint16_t) ((float)TPM0_MOD * (float)((MotorA)));
		TPM0_C3V = 0;
	}
	else
	{
		TPM0_C2V =0 ;
		TPM0_C3V =(uint16_t) ((float)TPM0_MOD * (float)((MotorA))) ;
	}
	
	
	if(DirB==1)
	{
		TPM0_C0V = (uint16_t) ((float)TPM0_MOD * (float)((MotorB)));
		TPM0_C1V = 0;	
	}
	else
	{
		TPM0_C0V = 0;
		TPM0_C1V = (uint16_t) ((float)TPM0_MOD * (float)((MotorB)));
	}
	
	



	
}






